// ======================================================================
// Copyright (C) 2013 Hell-Prototypes. / www.hellprototypes.com
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or (at
// your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
// ======================================================================
#include "hell_arm.h"
#include "ui_hell_arm.h"
#include <QPixmap>

#define HW_TIME_BASE            20 //unit: ns

#define PWM_DUTY_MIN            25000  // 0.5ms
#define PWM_DUTY_MAX            125000 // 2.5ms

#define SERVO_Z_AXIS_CH         0
#define SERVO_PICKER_ANGLE_CH   1
//===========================================================
#define USB_CMD_RESET           0x00
#define USB_CMD_RD_STATUS       0x01

#define USB_CMD_SET_SERVO       0x80
#define USB_CMD_SET_STEPMOTOR   0x81
#define USB_CMD_PUMP_CTRL       0x82

bool pap_thread::stepmotor_enable_ctrl(bool active)
{
    uint param = 1 << 27;

    if(active) {
        param |= 1 << 0;
    }

    if(usb_Ctrl_CMD(USB_CMD_SET_STEPMOTOR, param) < 0) {
        return false;
    }

    return true;
}

bool pap_thread::move_x_axis(int distance)
{
    uint param = 0;
    static int last_dir = 0;
    bool need_offset = false;

    if(distance < 0) {
        distance = -distance;
        if(last_dir > 0) {
            need_offset = true;
        }
    } else if(distance > 0) {
        param |= 1<<24;
        if(last_dir < 0) {
            need_offset = true;
        }
    } else {
        return true;
    }
    param |= 1<<25;// move x axis

    uint step = abs(distance);
    if(need_offset) {
        step += STEP_OFFSET_NUM;
    }
    //disp_system_msg(QString("X axis move step = %1, distance = %2").arg(step).arg(distance));
    if(step > STEPMOTOR_MAX_STEP_LIMIT) {
        emit disp_system_msg("move x axis fail, Over the Edge");
        return false;
    }

    param |= step & 0x1FFF;

    if(usb_Ctrl_CMD(USB_CMD_SET_STEPMOTOR, param) < 0) {
        return false;
    }

    return true;
}

bool pap_thread::move_y_axis(int distance)
{
    uint param = 0;
    static int last_dir = 0;
    bool need_offset = false;

    if(distance < 0) {
        distance = -distance;
        if(last_dir > 0) {
            need_offset = true;
        }
    } else if(distance > 0) {
        param |= 1<<24;
        if(last_dir < 0) {
            need_offset = true;
        }
    } else {
        return true;
    }
    //param |= 1<<25;// move x axis

    uint step = abs(distance);
    if(need_offset) {
        step += STEP_OFFSET_NUM;
    }
    //disp_system_msg(QString("Y axis move step = %1").arg(step));
    if(step > STEPMOTOR_MAX_STEP_LIMIT) {
        emit disp_system_msg("move y axis fail, Over the Edge");
        return false;
    }
    param |= step & 0x1FFF;

    if(usb_Ctrl_CMD(USB_CMD_SET_STEPMOTOR, param) < 0) {
        return false;
    }

    return true;
}

bool pap_thread::set_move_speed(int speed)
{
    if((speed >= 1) && (speed <= 10)) {
        m_move_speed = speed;
        uint speed_param = (1 << 26) + (((11 - speed - 1)/3 + 1)*1000000/HW_TIME_BASE)/8;// 1 - 10
        if(usb_Ctrl_CMD(USB_CMD_SET_STEPMOTOR, speed_param) < 0) {
            return false;
        }
    } else {
        return false;
    }
    return true;
}

int pap_thread::get_controller_status()
{
    char rd_buffer[4];
    int retry = 3, ret;

    while(retry--)  {
        if(usb_Ctrl_CMD(USB_CMD_RD_STATUS, 0) < 0) {
            ret = -1;
            msleep(10);
            continue;
        }
        ret = read_bulk_data(rd_buffer, 1);
        if(ret == 1) {
            return (uchar)rd_buffer[0];
        } else {
            msleep(10);
            ret = -2;
        }
    }

    emit disp_system_msg(QString("Get controller status fail, error code: %1").arg(ret));
    return ret;
}

bool pap_thread::move_arm_to(const iPoint &target_pos, int delay)
{
    int dx = target_pos.x - m_curr_arm_pos.x;
    int dy = target_pos.y - m_curr_arm_pos.y;

//    int speed = m_move_speed;
//    if( (delay > 0) && ((abs(dx) >= 10) || (abs(dy) >= 10))) {
//        if(speed > 5) {
//            if(!set_move_speed(speed)) {
//                return false;
//            }
//        }
//    }

    if(!move_x_axis(dx)) {
        return false;
    }
    if(!move_y_axis(dy)) {
        return false;
    }

    m_curr_arm_pos = target_pos;
    emit xy_axis_pos(target_pos.x, target_pos.y);
    //disp_system_msg(QString("m_curr_arm_pos.x = %1, m_curr_arm_pos.y = %2").arg(m_curr_arm_pos.x).arg(m_curr_arm_pos.y));

    int last_staus = 0;
    if(delay > 0) {
        int loop = delay * 10/ m_move_speed;
        while(loop > 0) {
            int status = get_controller_status();
            if(last_staus != status) {
                last_staus = status;
                emit controller_status_update(status);
            }
            if(status < 0) {
                return false;
            }
            if(!(status & 1)) {
                break;
            }
            msleep(5);
            loop--;

//            if(speed < m_move_speed) {//soft start
//                speed++;
//                if(!set_move_speed(speed)) {
//                    return false;
//                }
//            }
        }
        if(loop == 0) {
            return false;
        }
    }
    return true;
}

int pap_thread::angle_to_duty(double angle)
{
//    while(angle > PI)  angle -= PI;
//    while(angle < 0)     angle += PI;
    if((angle < 0) || (angle > PI)) {
        return -1;
    }

    int duty = (int)((angle/PI) * (PWM_DUTY_MAX - PWM_DUTY_MIN) + PWM_DUTY_MIN);

    if(duty > PWM_DUTY_MAX) {
        duty = PWM_DUTY_MAX;
    }
    if(duty < PWM_DUTY_MIN) {
        duty = PWM_DUTY_MIN;
    }

    return duty;
}

bool pap_thread::set_servo_angle(uint ch, double angle)
{
    int duty;
    uint pwm_param;

    ANGLE_PARAM_CHECK(angle);
    duty = angle_to_duty(angle);
    if(duty < 0) {
        return false;
    }

    switch(ch) {
    case SERVO_Z_AXIS_CH:
    case SERVO_PICKER_ANGLE_CH:
        pwm_param = (ch << 24) | (uint)duty;
        //disp_system_msg(QString("servo_angle: %1 = %2").arg(ch).arg((angle/PI)*180));
        break;
    default:
        return false;
        break;
    }
    if(usb_Ctrl_CMD(USB_CMD_SET_SERVO, pwm_param) < 0) {
        return false;
    }
    return true;
}

bool pap_thread::picker_move_to_top()
{
    if(!set_servo_angle(SERVO_Z_AXIS_CH , PI - 0.5)) {
        return false;
    }
    msleep(500);

    return true;
}

bool pap_thread::picker_move_up()
{
    if(!set_servo_angle(SERVO_Z_AXIS_CH , PI - 1.3)) {
        return false;
    }
    msleep(500);

    return true;
}

bool pap_thread::place_picker_move_down()
{
    if(!set_servo_angle(SERVO_Z_AXIS_CH , PI - 2.1)) {
        return false;
    }
    msleep(500);

    return true;
}

bool pap_thread::pick_picker_move_down()
{
    if(!set_servo_angle(SERVO_Z_AXIS_CH , PI - 2.3)) {
        return false;
    }
    msleep(500);

    return true;
}

bool pap_thread::turn_pump_on(bool on)
{
    if(usb_Ctrl_CMD(USB_CMD_PUMP_CTRL, on ? 1: 0) < 0) {
        return false;
    }

    return true;
}

bool pap_thread::set_picker_updown(double angle)
{
    if(!set_servo_angle(SERVO_Z_AXIS_CH , angle)) {
        return false;
    }
    msleep(300);
    return true;
}

bool pap_thread::set_picker_angle(double angle)
{
    if(!set_servo_angle(SERVO_PICKER_ANGLE_CH , angle)) {
        return false;
    }
    msleep(300);
    //emit disp_system_msg(QString("picker_angle = %1").arg(angle*180/PI));

    return true;
}

bool pap_thread::init_arm_position()
{
    if(usb_Ctrl_CMD(USB_CMD_RESET, 0) < 0) {
        emit disp_system_msg("Reset Contorler Fail");
        return false;
    }
    turn_pump_on(false);

    if(!set_picker_angle(PICKER_IDLE_ANGLE)) {
        emit disp_system_msg("Init picker angle Fail");
        return false;
    }

    if(!picker_move_up()) {
        emit disp_system_msg("Move picker up Fail");
        return false;
    }

    if(set_move_speed(m_move_speed)) {
        emit disp_system_msg("Arm move speed initialize SUCC");
        return true;
    } else {
        emit disp_system_msg("Arm move speed initialize FAIL");
    }

    return false;
}

bool pap_thread::move_arm_to_origin()
{
    if(!picker_move_up()) {
        return false;
    }

    iPoint target;

    target.x = 0;
    target.y = 0;
    if(move_arm_to(target, 1000)) {
        return true;
    } else {
        return false;
    }
}
bool pap_thread::move_cam_to_feeder_zone(const iPoint &zone_center)
{
    m_feeder_zone_center = zone_center;

    return move_arm_to(zone_center, 1000);
}
